#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>

#pragma pack(1) // 取消默认对齐，按单字节对齐 

#define DM_SERVO_1_ID                   (0x3)  // DM6265  RSY2C 云台关节电机
#define DM_SERVO_2_ID                   (0x4)  // DM6275  RSY2C 机械臂关节电机
#define DM_FEED_STATUS_SERVO_1_ID       (0x13)
#define DM_FEED_STATUS_SERVO_2_ID       (0x14)

#define DM_POSITON_SPEED_CTRL_MODE (0x100)

typedef enum {
  DM_SERVO_1,
  DM_SERVO_2,
  DM_SERVO_MAX
} dmservo_t;


typedef struct {
  uint8_t id : 4; // 舵机ID
  uint8_t err : 4; // 错误码
  uint8_t pos_h; // 位置高8位
  uint8_t pos_l; // 位置低8位
  uint8_t vel_h; // 速度高8位
  uint8_t touque_h : 4; // 扭矩高4位
  uint8_t vel_l : 4; // 速度低4位
  uint8_t touque_l; // 扭矩低8位
  uint8_t temp_mos; // 驱动mos温度, 单位：℃
  uint8_t temp_rotor; // 电机内部线圈温度, 单位：℃
} dmservo_feed_status_info_t;

typedef struct {
  uint8_t id;  // 舵机ID
  uint8_t err_code; // 错误码
  int16_t pos; // 舵机位置, 单位 rad  , 16位
  int16_t vel;  // 舵机速度, 单位 rad/s ， 12位
  int16_t touque; // 舵机力矩, 单位 N*m ,  12位
  uint8_t temp_mos; // 驱动mos温度, 单位：℃
  uint8_t temp_rotor; // 电机内部线圈温度, 单位：℃
} uuvlink_msg_dmservo_status_t;

#pragma pack()

float P_MIN=-12.5;
float P_MAX=12.5;

float V_MIN=-25;
float V_MAX=25;

float T_MIN=-100;
float T_MAX=100;

// 弧度转角度
float rad2deg(float rad) {
  return rad * 180 / 3.14159265358979323846;
}

float deg2rad(float deg) {
  return deg * 3.14159265358979323846 / 180;
}

float uint_to_float(int x_int, float x_min, float x_max, int bits){
    /// converts unsigned int to float, given range and number of bits ///
    float span = x_max - x_min;
    float offset = x_min;
    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}

float position,velocity,torque;
int p_int,v_int,t_int;
void serialdecodedata(uint8_t *Data) {
  p_int= (Data[1]<<8)|Data[2];
  v_int= (Data[3]<<4)|(Data[4]>>4);
  t_int= ((Data[4]&0xF)<<8)|Data[5];
  position = uint_to_float(p_int, P_MIN, P_MAX, 16); // (-12.5,12.5)
  velocity = uint_to_float(v_int, V_MIN, V_MAX, 12); // (-45.0,45.0)
  torque	= uint_to_float(t_int, T_MIN, T_MAX, 12); // (-18.0,18.0)

  printf("position: %f rad, velocity: %f rad/s, torque: %f\n", position, velocity, torque);
  printf("position: %f deg, velocity: %f deg/s, torque: %f\n", rad2deg(position), rad2deg(velocity), torque);
}


void ctrl_motor2(float	_pos,	float	_vel) {	
	uint8_t *pbuf,*vbuf;
  uint8_t Data[8] = {0};
	pbuf=(uint8_t*)&_pos;
  vbuf=(uint8_t*)&_vel;

 Data[0] = *pbuf;
 Data[1] = *(pbuf+1);
 Data[2] = *(pbuf+2);
 Data[3] = *(pbuf+3);
 Data[4] = *vbuf;
 Data[5] = *(vbuf+1);
 Data[6] = *(vbuf+2);
 Data[7] = *(vbuf+3);

  printf("pos-speed-ctrl send: %x %x %x %x %x %x %x %x\n", Data[0], Data[1], Data[2], Data[3], Data[4], Data[5], Data[6], Data[7]);
}


//  额定转速小于 3.7 rad/s

int main(int argc, char const *argv[]) {

  uint8_t ackData[8] = {0x14, 0x7F, 0xF7, 0x7F, 0xF8, 0xfe, 0x15, 0x13};
  serialdecodedata(ackData);

  // 0 rad   0.5 rad/s
  // ctrl_motor2(0, 0.5);

  // float angle_rad = 0.02;
  // printf("%f rad: %f deg\n", angle_rad, rad2deg(angle_rad));

  // float angle_deg = 1;  // deg/s
  // angle_rad = deg2rad(angle_deg);
  // printf("%f deg: %f rad\n", angle_deg, deg2rad(angle_deg));

  ctrl_motor2(deg2rad(-90), deg2rad(1.0));
  
  return 0;
}

